// ROS
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <memory>
#include <opencv2/core/mat.hpp>
#include <opencv2/videoio.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/utilities.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <string>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <thread>

#include "usb_cam_driver.hpp"

namespace usb_cam
{
class USBCamNode:public rclcpp::Node
{
public:

  explicit USBCamNode(const rclcpp::NodeOptions & options) : Node("usb_cam_node", options)
  {
    RCLCPP_INFO(this->get_logger(), "Starting USBCamNode!");

    //初始化相机
    std::string video_device_ = this->declare_parameter("video_device", "/dev/video0");
    RCLCPP_INFO(this->get_logger(), "Using video device: %s", video_device_.c_str());
    usb_cam = std::make_unique<UsbCamDriver>(video_device_);
    //设置相机参数
    cam_params.image_width = this->declare_parameter("image_width", 640);
    cam_params.image_height = this->declare_parameter("image_height", 480);
    cam_params.frame_rate = this->declare_parameter("frame_rate", 30.0);
    cam_params.auto_exposure = this->declare_parameter("auto_exposure", 0);
    cam_params.exposure_time = this->declare_parameter("exposure_time", 100);
    cam_params.gain = this->declare_parameter("gain", 16);
    usb_cam->setCamera(cam_params);
    


    //创建发布者
    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
    std::string topic_name_ = this->declare_parameter("topic_name", "image_raw");
    camera_pub_ = image_transport::create_camera_publisher(this, topic_name_, qos);

    frame_id = this->declare_parameter("frame_id", "usb_cam_frame");

    //初始化camera info
    loadCameraInfo();

    //参数通信回调
    params_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&USBCamNode::parametersCallback, this, std::placeholders::_1));

    capture_thread_ = std::thread(&USBCamNode::captureThread, this);

  }

  void captureThread()
  {
    while(rclcpp::ok())
    {
      std::lock_guard<std::mutex> lock(camera_mutex_); // 获取锁
      if(!usb_cam->get_frame(frame))
      {
        RCLCPP_ERROR(this->get_logger(), "Failed to capture frame from camera");
        break;
      }

      //Mat转换为ROS消息
      header.stamp = this->now();
      header.frame_id = frame_id;
      cv_bridge::CvImage cv_image(header, "bgr8", frame);
      cv_image.toImageMsg(image_msg_);

      camera_info_msg_.header = image_msg_.header;

      camera_pub_.publish(image_msg_, camera_info_msg_);
    }
  }

  ~USBCamNode() override
  {
    if (capture_thread_.joinable()) {
      capture_thread_.join();
    }
  }

  /**
    @brief 加载相机参数
  */
  void loadCameraInfo()
  {  
    // Load camera info
    std::string camera_name_ = this->declare_parameter("camera_name", "narrow_stereo");
    std::string camera_info_url = this->declare_parameter("camera_info_url", "package://usb_cam/config/camera_info.yaml");
    RCLCPP_INFO(this->get_logger(), "Using camera name: %s", camera_name_.c_str());
    camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
    if (camera_info_manager_->validateURL(camera_info_url)) {
      camera_info_manager_->loadCameraInfo(camera_info_url);
      camera_info_msg_ = camera_info_manager_->getCameraInfo();
    } else {
      RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s", camera_info_url.c_str());
    }

  }

  
  rcl_interfaces::msg::SetParametersResult parametersCallback(
    const std::vector<rclcpp::Parameter> & parameters)
  {
    std::lock_guard<std::mutex> lock(camera_mutex_); // 获取锁
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;
    for (const auto & param : parameters) {
      if (param.get_name() == "image_width") {
        cam_params.image_width = param.as_int();

      } else if (param.get_name() == "image_height") {
        cam_params.image_height = param.as_int();

      } else if (param.get_name() == "frame_rate") {
        cam_params.frame_rate = param.as_double();

      } else if (param.get_name() == "auto_exposure") {
        cam_params.auto_exposure = param.as_int();

      } else if (param.get_name() == "exposure_time") {
        cam_params.exposure_time = param.as_int();

      } else if (param.get_name() == "gain") {
        cam_params.gain = param.as_int();

      } else {
        result.successful = false;
        result.reason = "Unknown parameter: " + param.get_name();
      }
    }
    this->usb_cam->setCamera(cam_params);
    return result;
  }

private:

  std::unique_ptr<UsbCamDriver> usb_cam;
  cv::Mat frame;
  CameraParameters cam_params;

  std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;   //相机信息管理器

  //publish the camera image and info
  std_msgs::msg::Header header;
  std::string frame_id;
  
  image_transport::CameraPublisher camera_pub_;   //相机图像发布者
  sensor_msgs::msg::Image image_msg_;             //相机图像信息
  sensor_msgs::msg::CameraInfo camera_info_msg_;  //相机内参信息


  OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;

  // 线程回调
  std::thread capture_thread_;

  std::mutex camera_mutex_; // 互斥锁
};


} // namespace usb_cam

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(usb_cam::USBCamNode)